#include<Algorithm/poisson_construct_base.h>
#include<Eigen/IterativeLinearSolvers>
#include<Eigen/Sparse>
#include<vector>

using namespace Eigen;

extern double fx_dot_fx[5];
extern double fx_dot_Dfx[5];
extern double fx_dot_DDfx[5];

static inline int is_same_point(double *p1,double *p2)
{
    if(fabs(p1[0]-p2[0])<1e-6&&fabs(p1[1]-p2[1])<1e-6&&fabs(p1[2]-p2[2])<1e-6)
    {
        return 1;
    }
    return 0; 
}
static inline template_v* find_same_vertex(double* p,template_c* c)
{
    if(is_same_point(p,c->vertices[0]->point))
    {
        return c->vertices[0];
    }
    else if(is_same_point(p,c->vertices[1]->point))
    {
        return c->vertices[1];
    }
    else if(is_same_point(p,c->vertices[2]->point))
    {
        return c->vertices[2];
    }

    return NULL;
}

static template_c* create_cell_from_marching_cubes(Mesh*m,Node****tris,int len_grids, 
    TRIANGLE*t,int n)
{
    //printf("begin create one triangle\n");
    template_v*vs[3]={NULL};
    for(int l=0;l<3;l++)
    {
        for(int i=-1;i<2;i++)
        {
            for(int j=-1;j<2;j++)
            {
                for(int k=-1;k<2;k++)
                {
                    if(t->i+i>=0&&t->i+i<len_grids&&
                        t->j+j>=0&&t->j+j<len_grids&&
                        t->k+k>=0&&t->k+k<len_grids)
                    {
                        for(Node*nit=tris[t->i+i][t->j+j][t->k+k];nit!=NULL;nit=(Node*)(nit->Next))
                        {
                            TRIANGLE* t1=(TRIANGLE*)(nit->value);
                            template_c* c1=(template_c*)(t1->prop);
                            if(c1!=NULL)
                            {
                                vs[l]=find_same_vertex(t->p[l],c1);
                                if(vs[l]!=NULL)
                                {
                                    goto label;    
                                }

                            }

                        }

                    }
                    
                }
            }
        }

        label:
        if(vs[l]==NULL)
        {
            vs[l]=m->create_vertexv(m,t->p[l],3);
        }

    }

    template_c* c=m->create_cellv(m,vs,3);
    t->prop=c;
    if(c==NULL)
    {
        printf("create cell shibai %d\n",n);
        if(n==34)
        {
            template_f* f=m->get_facep(m,61039);
            printf("c1 :%d c2:%d\n",f->halffaces[0]->cell->id,f->halffaces[1]->cell->id);
        }
        printf("%d %d %d\n",vs[0]->id,vs[1]->id,vs[2]->id);
    }
    //printf("end create one triangle\n");

    return c;

}
static Mesh* create_mesh_from_marching_cubes(Node****tris,Node* tris1,int len_grids)
{
    Mesh* re=(Mesh*)malloc(sizeof(Mesh));
    Mesh_init(re);
    re->dimension=2;re->simplex=1;re->manifold_require=1;
    int num=0;
    for(Node*nit =tris1;nit!=NULL;nit=(Node*)(nit->Next))
    {
        TRIANGLE*t=(TRIANGLE*)(nit->value);
        // if(!is_illegal_tri(t))
        // {
        //     continue;
        // }
        template_c* c=create_cell_from_marching_cubes(re,tris,len_grids,t,num);
        if(c==NULL)
        {
            num++;
        }

    }
    return re;
}
static void  poisson_test_sparse_matrix(Poisson_Grid*pg )
{
    int len=node_size(pg->candidate);

    SparseMatrix<double,RowMajor> A(len,len);
    A.reserve(VectorXi::Constant(len,140));
    printf("begin sparse matrix %d \n",len);
    //std::unordered_map<MY_HashKey,double,MY_HashKey> unmap;

    int id=0,id1=0;
    int sum=0;double temp=0;

    for(Node* nit= pg->candidate;nit!=NULL;nit=(Node*)(nit->Next))
    {
        Poisson_Grid_Prop*pgp=(Poisson_Grid_Prop*)(nit->value);
        id=pgp->id;
        sum++;
        int i=pgp->indexs[0],j=pgp->indexs[1],k=pgp->indexs[2];

        for(int l=0;l<5;l++ )
        {
            for(int m=0;m<5;m++)
            {
                for(int n=0;n<5;n++)
                {
                    if(i+l-2<0||i+l-2>=pg->size||j+m-2<0||j+m-2>=pg->size||k+n-2<0||k+n-2>=pg->size)
                    {
                        continue;
                    }
                    if(pg->pgps[i+l-2][j+m-2][k+n-2]==NULL)
                    {
                        continue;
                    }
                    id1=pg->pgps[i+l-2][j+m-2][k+n-2]->id;
                    temp=0;
                    temp+=(fx_dot_DDfx[l]*fx_dot_fx[m]*fx_dot_fx[n]); 
                    temp+=(fx_dot_fx[l]*fx_dot_DDfx[m]*fx_dot_fx[n]); 
                    temp+=(fx_dot_fx[l]*fx_dot_fx[m]*fx_dot_DDfx[n]);
                    temp/=(pg->width*pg->width);
                    A.coeffRef(id,id1)=temp;

                }
            }
        }
    }      
    
    printf("beigin compress\n");
    //fill_sparse_matrix(A,unmap);
    A.makeCompressed();

    ConjugateGradient<SparseMatrix<double>,Eigen::Upper> solver;
    //solver.setMaxIterations(50);
    //Eigen::BiCGSTAB<Eigen::SparseMatrix<double>, Eigen::IncompleteLUT<double>> solver;
    //Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>>solver;
    double*bi= poisson_compute_b(pg );

    Eigen::VectorXd b(len);
    for(int i=0;i<len;i++)
    {
        b.coeffRef(i)=bi[i];
    }   

    printf("end b\n"); 

    free(bi);
      
    VectorXd x= solver.compute(A).solve(b); 
    pg->solve=(double*)malloc(sizeof(double)*len); 
    for(int i=0;i<len;i++)
    {
        pg->solve[i]=x.coeffRef(i);
    }
    printf(" 1sum:%d\n",sum);
}

Mesh* create_poisson_reconstruct_mesh(Node* node_v,int fen,int mfen)
{
    Poisson_Grid*pg=(Poisson_Grid*)malloc(sizeof(Poisson_Grid));
    poisson_grid_init(pg);

    //poisson_grid_init_with_depth(pg,depth);
    poisson_grid_init_with_accuracy(pg, fen);
    fill_normal_coeff(pg,node_v);

    poisson_test_sparse_matrix(pg);
    
    double iso=compute_average_poisson_iso(pg);
    printf("begin hrerere\n");
    // Node* node_tris=marching_cubes_generate_triangles(0.2,100,my_fun2,NULL);
    // Node* node_tris=marching_cubes_generate_triangles(iso,100,end_poisson_space_fun,pg);
    Node*node_tris1=NULL;

    //Node****node_tris=marching_cubes_generate_trianglesn(iso,100,end_poisson_space_fun,pg,&node_tris1);

    Node****node_tris=my_marching_cubes_generate_trianglesn(node_v,iso,mfen,end_poisson_space_fun,pg,&node_tris1);

    Mesh* m1= create_mesh_from_marching_cubes(node_tris,node_tris1,mfen);

    for(int i=0;i<mfen;i++)
    {
        for(int j=0;j<mfen;j++)
        {
            for(int k=0;k<mfen;k++)
            {
                free_node(node_tris[i][j][k]);
            }

            free(node_tris[i][j]);
        }
        free(node_tris[i]);
    }
    free(node_tris);
    poisson_grid_free(pg);

    free_node_value(node_tris1);
    free_node(node_tris1);
 
    return m1;
}